/* 
 * File:   main.c
 * Author: Bram
 *
 * Created on 1 januari 2015, 16:44
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <p33Fxxxx.h>


#pragma optimize = 0

unsigned int filterConfig[6] __attribute__((__space__(xmemory), __aligned__(4))) =
{
    64, //A1
    64, //A2
    508*32, //B1*32
    508, //B1
    256, // rounding 1
    16 // rounding 2
};

unsigned int filterValues[6] __attribute__((__space__(ymemory), __aligned__(4))) =
{
    0, // input
    0, // last-input
    0, // last-output
    0, // last-output % 32
    1, // rounding 1
    1 // rounding 2
};

unsigned int output;

void testDSP()
{
    asm(
        "mov _output, w5\n"                         // load (old) output
        "mov #_filterValues, w2\n"                  // load values array pointer
        "mov w5, [w2+4]\n"                          // values[2] = output

        "mov #_filterConfig, w8\n"                  // load config array pointer
        "mov #_filterValues, w10\n"                 // load values array pointer
        "clr A, [w8]+=2, w4, [w10]+=2, w6\n"        // clear accumulator A, prefetch data from arrays
        "mac w4*w6,A, [W8]+=2, W4, [W10]+=2, W6 \n" // A += config[0]*values[0]
        "mac w4*w6,A, [W8]+=2, W4, [W10]+=2, W6 \n" // A += config[1]*values[1]
        "mac w4*w6,A, [W8]+=2, W4, [W10]+=2, W6 \n" // A += config[2]*values[2]
        "mac w4*w6,A, [W8]+=2, W4, [W10]+=2, W6 \n" // A += config[3]*values[3]
        "mac w4*w6,A, [W8]+=2, W4, [W10]+=2, W6 \n" // A += config[4]*values[4] ==> ROUNDING + 256
        );
    asm(

        "sftac A, #9\n"                             // A >>= 9
        "mov [w2], w5\n"                            // load values[0]
        "mov w5, [w2+2]\n"                          // values[1] = values[0]

//        "mac w4*w6,A, [W8]+=2, W4, [W10]+=2, W6 \n" // A += config[5]*values[5] ==> ROUNDING + 16

        "mov ACCA, w5\n"                            // load accumulator LSB
        "and #0x1F, w5\n"                           // AND 0x1F
        "mov w5, [w2+6]\n"                          // values[3] = A(LSB) & 0x1F
        "sftac A, #5\n"                             // A >>= 5
        "mov ACCA, w2\n"                            // load accumulator LSB
        "mov w2, _output\n"                         // output = A(LSB)

        : : : "w2", "w4", "w5", "w6", "w8","w10"                 // compiler hints? pushes w8/w10 to stack
    );
}

float input_f;
unsigned int input_i;

unsigned int delay = 0xF000;

int main(int argc, char** argv)
{
    float i = 0;
    CORCONbits.US = 1; //DSP unsigned
    CORCONbits.IF = 1; //Integer mode
    CORCONbits.SATA = 1; //Saturation enabled
    CORCONbits.RND = 0; //No round?
    while (1)
    {
        input_f = sinf(i)*200.0f+250.0f;
        input_i = input_f;
        filterValues[0] = input_i;
        testDSP();
        i+= 0.001f;
        if (i > 3.14156f*2) i -= 3.14156f*2;

        int j = 0;
        while (j++ < 1000);
    }

    return 0;
}

/* EasyCODE ) */
/* EasyCODE ( 0
ERROR TRAPS */
/* EasyCODE F */
void __attribute__((interrupt, no_auto_psv)) _OscillatorFail(void)
{
    #ifdef __DEBUG
        while (1)
        {
            asm("NOP");
        }
    #endif
    asm("RESET");
}

/* EasyCODE F */
void __attribute__((interrupt, no_auto_psv)) _AddressError(void)
{
    #ifdef __DEBUG
        while (1)
        {
            asm("NOP");
        }
    #endif
    asm("RESET");
}

/* EasyCODE F */
void __attribute__((interrupt, no_auto_psv)) _StackError(void)
{
    #ifdef __DEBUG
        while (1)
        {
            asm("NOP");
        }
    #endif
    asm("RESET");
}

/* EasyCODE F */
void __attribute__((interrupt, no_auto_psv)) _MathError(void)
{
    #ifdef __DEBUG
        while (1)
        {
            asm("NOP");
        }
    #endif
    asm("RESET");
}



